//------------------------------------------------------------------------------
//
// Motion Recorder for Lynx 6 Arm
//
// Written Nov-2007 by Trevor Taylor, Software Technology, Australia
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using motionrecorder = ProMRDS.Robotics.MotionRecorder;

// Added for Windows Forms support
using Microsoft.Ccr.Adapters.WinForms;
// Needed for AxisAngle, Vector3, etc.
using Microsoft.Robotics.PhysicalModel.Proxy;
using physicalmodel = Microsoft.Robotics.PhysicalModel;
using arm = Microsoft.Robotics.Services.ArticulatedArm.Proxy;
using lynx = ProMRDS.Robotics.Lynxmotion.Lynx6Arm.Proxy;
using W3C.Soap;

namespace ProMRDS.Robotics.MotionRecorder
{
    
    /// <summary>
    /// Implementation class for MotionRecorder
    /// </summary>
    [DisplayName("MotionRecorder")]
    [Description("The MotionRecorder Service")]
    [Contract(Contract.Identifier)]
    public class MotionRecorderService : DsspServiceBase
    {
        /// <summary>
        /// _state
        /// </summary>
        [InitialStatePartner(Optional = true)]
        private MotionRecorderState _state = new MotionRecorderState();

        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/MotionRecorder", AllowMultipleInstances=false)]
        private MotionRecorderOperations _mainPort = new MotionRecorderOperations();

        //[Partner("Lynx6Arm", Contract = lynx.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting, Optional = false)]
        [Partner("Lynx6Arm", Contract = arm.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting, Optional = false)]
        private arm.ArticulatedArmOperations _lynxPort = new arm.ArticulatedArmOperations();

        // This port receives events from the user interface
        FromWinformEvents _fromWinformPort = new FromWinformEvents();
        // This is the instance of the Winform
        MotionRecorderUI _motionRecorderUI = null;


        // Names of all the joints from the arm service
        string[] _jointNames = null;

        // Joint angles for the Reset and Park commands
        float[] _resetAngles = { 0, 0, 0, 0, 0, 0 };
        float[] _parkAngles = { 0, 20, -50, 30, 0, 0 };


        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public MotionRecorderService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
			base.Start();
			// Add service specific initialization here.

            if (_state == null)
                CreateDefaultState();

            // Add the winform message handler to the interleave
            MainPortInterleave.CombineWith(new Interleave(
                new TeardownReceiverGroup(),
                new ExclusiveReceiverGroup
                (
                    Arbiter.ReceiveWithIterator<FromWinformMsg>(true, _fromWinformPort, OnWinformMessageHandler)
                ),
                new ConcurrentReceiverGroup
                (
                )
            ));

            // Create the user interface form
            WinFormsServicePort.Post(new Microsoft.Ccr.Adapters.WinForms.RunForm(CreateForm));
        }

        // Create the state using default settings
        void CreateDefaultState()
        {
            if (_state == null)
            {
                _state = new MotionRecorderState();
                //_state.Joints = new Dictionary<string, DOFDesc>();
            }
        }

        #region WinForms Communication

        // Create the UI form
        System.Windows.Forms.Form CreateForm()
        {
            return new MotionRecorderUI(_fromWinformPort);
        }

        // Process messages from the UI Form
        IEnumerator<ITask> OnWinformMessageHandler(FromWinformMsg msg)
        {
            switch (msg.Command)
            {
                case FromWinformMsg.MsgEnum.Loaded:
                    // Windows form is ready to go
                    _motionRecorderUI = (MotionRecorderUI)msg.Object;
                    SpawnIterator(EnumerateJoints);
                    // Fudge a GetPose to update the Form initially
                    //_fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.GetPose, null, null));
                    break;

                case FromWinformMsg.MsgEnum.Exit:
                    // Tell ourselves to shut down
                    _mainPort.Post(new DsspDefaultDrop());
                    ControlPanelPort.Shutdown();
                    break;

                case FromWinformMsg.MsgEnum.MoveToPosition:
                    {
                        // move the arm to the specified position
                        MoveToPositionParameters moveParams = (MoveToPositionParameters)msg.Object;
                        yield return Arbiter.Choice(
                            MoveToPosition(moveParams.X, moveParams.Y, moveParams.Z,
                            moveParams.GripAngle, moveParams.GripRotation, moveParams.Grip, moveParams.Time),
                            delegate(SuccessResult s) { },
                            delegate(Exception e)
                            {
                                WinFormsServicePort.FormInvoke(
                                    delegate()
                                    {
                                        _motionRecorderUI.SetErrorText(e.Message);
                                    });
                            }
                        );

                        break;
                    }

                case FromWinformMsg.MsgEnum.MoveJoint:
                    onMoveJoint((MoveJointParameters)msg.Object);
                    break;

                case FromWinformMsg.MsgEnum.MoveMultipleJoints:
                    onMoveMultipleJoints((MoveMultipleJointParameters)msg.Object);
                    break;

                case FromWinformMsg.MsgEnum.GetPose:
                    SpawnIterator(GetArmPoseHandler);
                    break;

                case FromWinformMsg.MsgEnum.Reset:
                    ResetArmPosition();
                    break;

                case FromWinformMsg.MsgEnum.Park:
                    SpawnIterator(ParkArm);
                    break;
            }
        }

        // Get a list of the names of all the arm joints
        // NOTE: The order of the joints MUST be as follows:
        // Base
        // Shoulder
        // Elbow
        // Wrist
        // Wrist Rotate
        // Gripper
        // The only reason for getting the names here is so that they can
        // be used when moving the joints.
        IEnumerator<ITask> EnumerateJoints()
        {
            int counter = 0;
            arm.ArticulatedArmState armState = null;

            // Get the arm state because it contains the list of joints
            // Wait until we get a response from the arm service
            // If it is not ready, then null will be returned
            // However, we don't want to do this forever!
            while (armState == null)
            {
                // Get the arm state which contains a list of all the joints
                yield return Arbiter.Choice(
                    _lynxPort.Get(new GetRequestType()),
                    delegate(arm.ArticulatedArmState state) { armState = state; },
                    delegate(Fault f) { LogError(f); }
                );
                // This is not good error handling!!!
                if (armState == null)
                {
                    counter++;
                    if (counter > 20)
                    {
                        Console.WriteLine("***** Could not get arm state. Is the arm turned on? *****");
                        //throw (new Exception("Could not get arm state!"));
                        yield break;
                    }

                    // Wait a little while before trying again ...
                    yield return Arbiter.Receive(false, TimeoutPort(500), delegate(DateTime now) { });

                }
            }

            // Get the arm state AGAIN because the first time seems to give
            // bad data -- don't know why
            yield return Arbiter.Choice(
                _lynxPort.Get(new GetRequestType()),
                delegate(arm.ArticulatedArmState state) { armState = state; },
                delegate(Fault f) { LogError(f); }
            );

            // Now that we have the current arm state,
            // we can initialize the joint angles in the Form
            SetUIJointAngles(armState);

            // Create the new list of names
            _jointNames = new string[armState.Joints.Count];
            for (int i = 0; i < armState.Joints.Count; i++)
            {
                _jointNames[i] = armState.Joints[i].State.Name;
            }

            // Now update the names in the Form so that it can do
            // whatever it needs to with them as well
            WinFormsServicePort.FormInvoke(delegate()
            {
                _motionRecorderUI.ReplaceArticulatedArmJointList(_jointNames);
            });

            yield break;
        }

        // Set the joint angles in the Form based on the current arm state
        void SetUIJointAngles(arm.ArticulatedArmState armState)
        {
            float baseAngle, shoulderAngle, elbowAngle, wristAngle, wristRotateAngle, gripperAngle;
            AxisAngle currentAngle;
            currentAngle = ProxyConversion.ToProxy(physicalmodel.Quaternion.ToAxisAngle(
                                    ProxyConversion.FromProxy(armState.Joints[(int)lynx.JointNumbers.Base].State.Angular.DriveTargetOrientation)));
            baseAngle = Conversions.RadiansToDegrees(currentAngle.Angle) * Math.Sign(currentAngle.Axis.X);
            currentAngle = ProxyConversion.ToProxy(physicalmodel.Quaternion.ToAxisAngle(
                                    ProxyConversion.FromProxy(armState.Joints[(int)lynx.JointNumbers.Shoulder].State.Angular.DriveTargetOrientation)));
            shoulderAngle = Conversions.RadiansToDegrees(currentAngle.Angle) * Math.Sign(currentAngle.Axis.X);
            currentAngle = ProxyConversion.ToProxy(physicalmodel.Quaternion.ToAxisAngle(
                                    ProxyConversion.FromProxy(armState.Joints[(int)lynx.JointNumbers.Elbow].State.Angular.DriveTargetOrientation)));
            elbowAngle = Conversions.RadiansToDegrees(currentAngle.Angle) * Math.Sign(currentAngle.Axis.X);
            currentAngle = ProxyConversion.ToProxy(physicalmodel.Quaternion.ToAxisAngle(
                                    ProxyConversion.FromProxy(armState.Joints[(int)lynx.JointNumbers.Wrist].State.Angular.DriveTargetOrientation)));
            wristAngle = Conversions.RadiansToDegrees(currentAngle.Angle) * Math.Sign(currentAngle.Axis.X);
            currentAngle = ProxyConversion.ToProxy(physicalmodel.Quaternion.ToAxisAngle(
                                    ProxyConversion.FromProxy(armState.Joints[(int)lynx.JointNumbers.WristRotate].State.Angular.DriveTargetOrientation)));
            wristRotateAngle = Conversions.RadiansToDegrees(currentAngle.Angle) * Math.Sign(currentAngle.Axis.X);
            currentAngle = ProxyConversion.ToProxy(physicalmodel.Quaternion.ToAxisAngle(
                                    ProxyConversion.FromProxy(armState.Joints[(int)lynx.JointNumbers.Gripper].State.Angular.DriveTargetOrientation)));
            gripperAngle = Conversions.RadiansToDegrees(currentAngle.Angle) * Math.Sign(currentAngle.Axis.X);

            // Update the details in the Form
            WinFormsServicePort.FormInvoke(delegate()
                {
                    _motionRecorderUI.SetSliders(
                        baseAngle,
                        shoulderAngle,
                        elbowAngle,
                        wristAngle,
                        wristRotateAngle,
                        gripperAngle);
                }
            );
        }

        #endregion

        // This method is executed if the MoveTo method fails
        void ShowError(Exception e)
        {
            Console.WriteLine(e.Message);
        }

        // This method puts the arm into its initial position
        void ResetArmPosition()
        {
            // Raise the grippers away from the working surface
            MoveMultipleJoints(_jointNames, _resetAngles);
        }

        // This method puts the arm into its parked position
        public IEnumerator<ITask> ParkArm()
        {
            // Raise the grippers away from the working surface
            MoveMultipleJoints(_jointNames, _resetAngles);

            // Pause for a moment
            yield return Arbiter.Receive(false, TimeoutPort(1500), delegate(DateTime now) { });

            // Move to the parked position
            MoveMultipleJoints(_jointNames, _parkAngles);
        }

        // This method calculates the joint angles necessary to place the arm into the 
        // specified position.  The arm position is specified by the X,Y,Z coordinates
        // of the gripper tip as well as the angle of the grip, the rotation of the grip, 
        // and the open distance of the grip.  The motion is completed in the 
        // specified time.
        public SuccessFailurePort MoveToPosition(
            float mx, // x position
            float my, // y position
            float mz, // z position
            float p, // angle of the grip
            float w, // rotation of the grip
            float grip, // distance the grip is open
            float time) // time to complete the movement
        {
            // Create a request
            // What a pain in the neck!!!
            // All because standard classes cannot be assigned to their proxies!
            arm.SetEndEffectorPoseRequest req = new arm.SetEndEffectorPoseRequest();
            Pose rp = new Pose();
            Vector3 v = new Vector3(mx, my, mz);
            rp.Position = v;
            physicalmodel.Quaternion q = new physicalmodel.Quaternion();
            q = physicalmodel.Quaternion.FromAxisAngle(1, 0, 0, Conversions.DegreesToRadians(p));
            Quaternion qp = new Quaternion(q.X, q.Y, q.Z, q.W);
            rp.Orientation = qp;
            req.EndEffectorPose = rp;
            _lynxPort.SetEndEffectorPose(req);

            // Set the positions of the wrist rotate and the gripper as well
            MoveJoint(_jointNames[(int)lynx.JointNumbers.WristRotate], w);
            MoveJoint(_jointNames[(int)lynx.JointNumbers.Gripper], grip);

            // Now return success
            SuccessFailurePort responsePort = new SuccessFailurePort();
            responsePort.Post(new SuccessResult());
            return responsePort;

        }

        // This is the most basic joint movement
        // NOTE: It does NOT stop and wait for the joint!
        // If you send a flood of joint movement requests, then they will
        // overwrite one another.
        void MoveJoint(string name, float angle)
        {
            arm.SetJointTargetPoseRequest req = new arm.SetJointTargetPoseRequest();
            req.JointName = name;
            AxisAngle aa = new AxisAngle(
                new Vector3(1, 0, 0),
                (float)(Math.PI * ((double)angle / 180)));

            req.TargetOrientation = aa;

            _lynxPort.SetJointTargetPose(req);
        }

        void onMoveJoint(MoveJointParameters onApply)
        {
            MoveJoint(onApply.Name, (float)onApply.Angle);
        }

        // Move several joints at the same time
        // NOTE: This is NOT an orchestrated move in any sense!
        // The joints will all move independently and arrive at their
        // target angles whenever they can. Obviously, joints that do
        // not have far to move will get there first.
        // It is OK to specify a joint with the same angle as it currently
        // has because it just won't move.
        // However, it is NOT necessary to specify all of the joints on
        // the robot. You can move a subset of joints if you want to.
        void MoveMultipleJoints(string[] jointNames, float[] jointAngles)
        {
            int i;
            for (i = 0; i < jointNames.Length && i < jointAngles.Length; i++)
            {
                MoveJoint(jointNames[i], jointAngles[i]);
            }
        }

        void onMoveMultipleJoints(MoveMultipleJointParameters onApply)
        {
            MoveMultipleJoints(onApply.Names, onApply.Angles);
        }

        // Get Arm Pose
        IEnumerator<ITask> GetArmPoseHandler()
        {
            arm.GetEndEffectorPoseResponse armPose = null;

            if (_lynxPort == null)
                yield break;

            // Get the current arm pose
            yield return Arbiter.Choice(
                _lynxPort.GetEndEffectorPose(new arm.GetEndEffectorPoseRequest()),
                delegate(arm.GetEndEffectorPoseResponse pose) { armPose = pose; },
                delegate(Fault f) { LogError(f); }
            );

            if (armPose == null)
                yield break;

            // For some reason the pose always seems to be out of synch
            // so get it a second time to be sure
            yield return Arbiter.Choice(
                _lynxPort.GetEndEffectorPose(new arm.GetEndEffectorPoseRequest()),
                delegate(arm.GetEndEffectorPoseResponse pose) { armPose = pose; },
                delegate(Fault f) { LogError(f); }
            );

            if (armPose == null)
                yield break;

            // Update the details in the Form
            WinFormsServicePort.FormInvoke(delegate()
                {
                    _motionRecorderUI.UpdateArmPose(armPose.EndEffectorPose);
                }
            );

            arm.ArticulatedArmState armState = null;

            // Get the arm state which contains a list of all the joints
            yield return Arbiter.Choice(
                _lynxPort.Get(new GetRequestType()),
                delegate(arm.ArticulatedArmState state) { armState = state; },
                delegate(Fault f) { LogError(f); }
            );

            // If nothing was returned, tough luck!
            // This is not good error handling!!!
            if (armState == null)
                yield break;

            // Now update the sliders on the Form
            SetUIJointAngles(armState);

            yield break;
        }


        #region Operation Handlers

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        #endregion

    }

    // Utility methods combined into a single class

    public class Conversions
    {
        public static float DegreesToRadians(float degrees)
        {
            return (float)(degrees * Math.PI / 180);
        }

        public static float RadiansToDegrees(float radians)
        {
            return (float)(radians * 180 / Math.PI);
        }

        public static float InchesToMeters(float inches)
        {
            return (float)(inches * 0.0254);
        }

    }

    public static class ProxyConversion
    {
        public static physicalmodel.Vector3 FromProxy(Vector3 source)
        {
            return new physicalmodel.Vector3(source.X, source.Y, source.Z);
        }

        public static physicalmodel.Quaternion FromProxy(Quaternion source)
        {
            return new physicalmodel.Quaternion(source.X, source.Y, source.Z, source.W);
        }

        public static physicalmodel.AxisAngle FromProxy(AxisAngle source)
        {
            return new physicalmodel.AxisAngle(FromProxy(source.Axis), source.Angle);
        }

        public static Vector3 ToProxy(physicalmodel.Vector3 source)
        {
            return new Vector3(source.X, source.Y, source.Z);
        }

        public static Quaternion ToProxy(physicalmodel.Quaternion source)
        {
            return new Quaternion(source.X, source.Y, source.Z, source.W);
        }

        public static AxisAngle ToProxy(physicalmodel.AxisAngle source)
        {
            return new AxisAngle(ToProxy(source.Axis), source.Angle);
        }
    }

}
